#ifndef __LIDAR2D_UNDISTORTION_H__
#define __LIDAR2D_UNDISTORTION_H__


#include "perception/sensors/base/sensor_base.h"
#include "perception/sensors/convert/msg_convert.h"

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <iostream>
#include <vector>
#include <cmath>
#include <chrono>
#include <boost/circular_buffer.hpp>
// https://blog.csdn.net/elephant_why/article/details/107131518
// https://github.com/elewu/2d_lidar_undistortion

using namespace std;
using namespace chrono;
typedef boost::circular_buffer<sensor_msgs::Imu::Ptr> ImuCircularBuffer;

class Lidar2DUndistortion{
    public:
        Lidar2DUndistortion();
        ~Lidar2DUndistortion() = default;

        // 设置传感器参数数据
        void setParams(const SensorParams& sensor_params);

        // 添加IMU数据
        void addImuData(sensor_msgs::Imu::Ptr data_ptr);
        // 添加里程计数据
        void addOdomData(nav_msgs::Odometry::Ptr data_ptr);

        geometry_msgs::Quaternion imuToCurrentQuaternion(geometry_msgs::Quaternion quat, float yaw_offset);
        Eigen::Quaternionf imuToEigenQuaternion(geometry_msgs::Quaternion quat, float yaw_offset);
        bool getLaserPose(int point_index, ros::Time& timestamp, Eigen::Quaternionf& quat_out);

        // 获取去除畸变数据 相对于雷达坐标系
        pcl::PointCloud<pcl::PointXYZ>::Ptr getLidarData(sensor_msgs::LaserScan::Ptr data_input);
        
        // 点云坐标转换
        pcl::PointCloud<pcl::PointXYZ>::Ptr transformPCL(pcl::PointCloud<pcl::PointXYZ>::Ptr data_input);

        // 是否初始化
        inline bool isInit() const{
            return init_params_;
        }

        // 获取传感器参数
        inline SensorParams getParams(){
            return sensor_params_;
        }

    private:
        SensorParams sensor_params_;
        bool init_params_;
        int imu_max_mums_;
        ImuCircularBuffer imu_cache_buffer_;
        double lidar_delay_time_;
        double imu_frequency_;
};


#endif